package org.chorus.utils;

import java.text.*;               // For decimal place formatting.

//  ***************************************************************************************************
/** Class containing Matrix processing operations. Includes general matrix operations such as addition,
 *  multiplication, and inverses, dot and cross products. Also includes  operations optimised for 3D 
 *  vectors. Some of these routines (e.g. vMatch) were converted from the vector maths utilities in C, 
 *  written by Gustav Taxen, 1998.
 * @author Jo Wood, some original code by Gustav Taxen.
 * @version 2.3.1, 17th July, 2012.             
 */
//  ***************************************************************************************************

/* This file is part of the Chorus river bed analysis software package. Chorus is free software:
 * you can redistribute it and/or modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation, either version 3 of the License, or (at your
 * option) any later version.
 * 
 * Chorus is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
 * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
 * See the GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License along with this
 * source code (see COPYING included with this source code). If not, see 
 * http://www.gnu.org/licenses/.
 */

public class Matrix
{   
	// --------------------------------- Static Methods ------------------------------------   

	/** Returns the transpose of a 2D matrix of any size. Useful when creating normal 
	 *  equations for least squares. 
	 *  @param m Input matrix
	 *  @return Transpose of the given matrix. 
	 */
	public static float[][] mTranspose(float[][] m)
	{
		int numRows =  m.length;
		int numCols = m[0].length;

		float[][] transpose = new float[numCols][numRows];

		for (int row=0; row<numCols; row++)
		{
			for (int col=0; col<numRows; col++)
			{
				transpose[row][col] = m[col][row];
			}
		}
		return transpose;   
	}

	/** Returns the transpose of a 2D matrix of any size. Useful when creating normal
	 *  equations for least squares. Double precision version.
	 *  @param m Input matrix
	 *  @return Transpose of the given matrix. 
	 */
	public static double[][] mTranspose(double[][] m)
	{
		int numRows =  m.length;
		int numCols = m[0].length;

		double[][] transpose = new double[numCols][numRows];

		for (int row=0; row<numCols; row++)
		{
			for (int col=0; col<numRows; col++)
			{
				transpose[row][col] = m[col][row];
			}
		}
		return transpose;   
	}

	/** Adds one (3D) vector to another.
	 *  @param u First vector.
	 *  @param v Second vector.
	 *  @return Sum of the two vectors (u+v).
	 */
	public static float[] vAdd(float[] u, float[] v)
	{
		float add[] = new float[3];
		add[0] = u[0] + v[0];
		add[1] = u[1] + v[1];
		add[2] = u[2] + v[2];
		return add;
	}
	
	/** Adds one (3D) vector to another. Double precision version.
	 *  @param u First vector.
	 *  @param v Second vector.
	 *  @return Sum of the two vectors (u+v).
	 */
	public static double[] vAdd(double[] u, double[] v)
	{
		double add[] = new double[3];
		add[0] = u[0] + v[0];
		add[1] = u[1] + v[1];
		add[2] = u[2] + v[2];
		return add;
	}

	/** Calculates the angle between two (3D) vectors u and v.
	 *  @param u First vector.
	 *  @param v Second vector.
	 *  @return Angle between the two vectors in radians.
	 */    
	public static float vAngle(float[] u, float[] v)
	{
		float uLen = vLength(u);
		float vLen = vLength(v);

		if ((uLen==0) || (vLen==0))
		{
			System.err.println("Warning: Can't calculate angle with zero length vector");
		}  
		return (float)Math.acos(vDot(u,v) / uLen*vLen);
	}

	/** Calculates the angle between two (3D) vectors u and v. Double precision version.
	 *  @param u First vector.
	 *  @param v Second vector.
	 *  @return Angle between the two vectors in radians.
	 */    
	public static double vAngle(double[] u, double[] v)
	{
		double uLen = vLength(u);
		double vLen = vLength(v);

		if ((uLen==0) || (vLen==0))
		{
			System.err.println("Warning: Can't calculate angle with zero length vector");
		} 
		return Math.acos(vDot(u,v) / uLen*vLen);
	}

	/** Produces a copy of a given (4x4) matrix.
	 *  @param m Matrix to copy.
	 *  @return Copy of original matrix.
	 */
	public static float[][] mCopy(float[][] m)
	{
		float [][] newMatrix = new float[4][4];

		for (int r=0; r<4; r++)
		{
			for (int c=0; c<4; c++)
			{
				newMatrix[r][c] = m[r][c];
			}
		}
		return newMatrix;
	}

	/** Produces a copy of a given (4x4) matrix. Double precision version
	 *  @param m Matrix to copy.
	 *  @return Copy of original matrix.
	 */
	public static double[][] mCopy(double[][] m)
	{
		double [][] newMatrix = new double[4][4];
		for (int r=0; r<4; r++)
		{
			for (int c=0; c<4; c++)
			{
				newMatrix[r][c] = m[r][c];
			}
		}
		return newMatrix;
	}   

	/** Produces a copy of a given (3D) vector.
	 *  @param v Vector to copy.
	 *  @return Copy of original vector.
	 */
	public static float[] vCopy(float[] v)
	{
		float [] newVect = new float[3];
		newVect[0] = v[0];
		newVect[1] = v[1];
		newVect[2] = v[2];
		return newVect;
	}

	/** Produces a copy of a given (3D) vector. Double precision version.
	 *  @param v Vector to copy.
	 *  @return Copy of original vector.
	 */
	public static double[] vCopy(double[] v)
	{
		double [] newVect = new double[3];
		newVect[0] = v[0];
		newVect[1] = v[1];
		newVect[2] = v[2];
		return newVect;
	}

	/** Calculates the cross product of two (3D) vectors.
	 *  @param u First vector.
	 *  @param v Second vector.
	 *  @return Cross product of the two vectors.
	 */
	public static float[] vCross(float[] u, float[] v)
	{
		float cross[] = new float[3];

		cross[0] = (u[1] * v[2]) - (u[2] * v[1]);
		cross[1] = (u[2] * v[0]) - (u[0] * v[2]);
		cross[2] = (u[0] * v[1]) - (u[1] * v[0]);

		return cross;
	}

	/** Calculates the cross product of two (3D) vectors.
	 *  @param x1 First vector x coordinate.
	 *  @param y1 First vector y coordinate.
	 *  @param z1 First vector z coordinate.
	 *  @param x2 Second vector x coordinate.
	 *  @param y2 Second vector y coordinate.
	 *  @param z2 Second vector z coordinate.
	 *  @return Cross product of the two vectors.
	 */
	public static float[] vCross(float x1, float y1, float z1, float x2, float y2, float z2)
	{
		float cross[] = new float[3];
		cross[0] = (y1 * z2) - (z1 * y2);
		cross[1] = (z1 * x2) - (x1 * z2);
		cross[2] = (x1 * y2) - (y1 * x2);
		return cross;
	}

	/** Calculates the cross product of two (3D) vectors. Double precision version.
	 *  @param u First vector.
	 *  @param v Second vector.
	 *  @return Cross product of the two vectors.
	 */
	public static double[] vCross(double[] u, double[] v)
	{
		double cross[] = new double[3];
		cross[0] = (u[1] * v[2]) - (u[2] * v[1]);
		cross[1] = (u[2] * v[0]) - (u[0] * v[2]);
		cross[2] = (u[0] * v[1]) - (u[1] * v[0]);
		return cross;
	}

	/** Calculates the cross product of two (3D) vectors. Double precision version.
	 *  @param x1 First vector x coordinate.
	 *  @param y1 First vector y coordinate.
	 *  @param z1 First vector z coordinate.
	 *  @param x2 Second vector x coordinate.
	 *  @param y2 Second vector y coordinate.
	 *  @param z2 Second vector z coordinate.
	 *  @return Cross product of the two vectors.
	 */
	public static double[] vCross(double x1, double y1, double z1, double x2, double y2, double z2)
	{
		double cross[] = new double[3];
		cross[0] = (y1 * z2) - (z1 * y2);
		cross[1] = (z1 * x2) - (x1 * z2);
		cross[2] = (x1 * y2) - (y1 * x2);
		return cross;
	}

	/** Displays the contents of a 2D matrix of any size to maximum number of decimal places.
	 * @param m Matrix to display.
	 */
	public static void mDisplay(float[][] m)
	{
		int numRows = m.length;
		int numCols = m[0].length;

		for (int r=0;r<numRows;r++)
		{
			System.out.print("[");

			for (int c=0;c<numCols-1;c++)
			{
				System.out.print(m[r][c]+",");
			}  
			System.out.println(m[r][numCols-1]+"]");   
		}
	}

	/** Displays the contents of a 2D matrix of any size to maximum number of decimal places. 
	 *  Double precision version.
	 *  @param m Matrix to display.
	 */
	public static void mDisplay(double[][] m)
	{
		int numRows = m.length;
		int numCols = m[0].length;

		for (int r=0;r<numRows;r++)
		{
			System.out.print("[");

			for (int c=0;c<numCols-1;c++)
			{
				System.out.print(m[r][c]+",");
			}   
			System.out.println(m[r][numCols-1]+"]");   
		}
	}

	/** Displays the contents of a 2D matrix of any size to a given number of decimal places.
	 *  @param m Matrix to display.
	 *  @param dp Number of decimal places to display.
	 */
	public static void mDisplay(float[][] m, int dp)
	{
		NumberFormat nFormat = NumberFormat.getInstance();
		nFormat.setMaximumFractionDigits(dp);
		nFormat.setMinimumFractionDigits(dp);
		String number;

		int numRows = m.length;
		int numCols = m[0].length;

		for (int r=0;r<numRows;r++)
		{
			System.out.print("[");
			for (int c=0; c<numCols; c++)
			{
				number = nFormat.format(m[r][c]);
				if (number.startsWith("-"))
				{
					System.out.print(number);
				}
				else
				{
					System.out.print(" "+number);
				}
				if (c<numCols-1)
				{
					System.out.print(",");
				}
				else
				{
					System.out.println("]");
				}
			}    
		}
	}

	/** Displays the contents of a 2D matrix of any size to a given number of decimal places. 
	 *  Double precision version.
	 *  @param m Matrix to display.
	 *  @param dp Number of decimal places to display.
	 */
	public static void mDisplay(double[][] m, int dp)
	{
		NumberFormat nFormat = NumberFormat.getInstance();
		nFormat.setMaximumFractionDigits(dp);
		nFormat.setMinimumFractionDigits(dp);
		String number;

		int numRows = m.length;
		int numCols = m[0].length;

		for (int r=0;r<numRows;r++)
		{
			System.out.print("[");
			for (int c=0; c<numCols; c++)
			{
				number = nFormat.format(m[r][c]);
				if (number.startsWith("-"))
				{
					System.out.print(number);
				}
				else
				{
					System.out.print(" "+number);
				}
				if (c<numCols-1)
				{
					System.out.print(",");
				}
				else
				{
					System.out.println("]");  
				}
			}  
		}
	}

	/** Displays the contents of a 1D vector of any size.
	 *  @param v Vector to display.
	 */
	public static void vDisplay(float[] v)
	{
		System.out.print("[");
		for (int i=0;i<v.length-1;i++)
		{
			System.out.print(v[i]+",");
		}
		System.out.println(v[v.length-1]+"]");   
	}

	/** Displays the contents of a 1D vector of any size. Double precision version.
	 *  @param v Vector to display.
	 */
	public static void vDisplay(double[] v)
	{
		System.out.print("[");
		for (int i=0;i<v.length-1;i++)
		{
			System.out.print(v[i]+",");
		}
		System.out.println(v[v.length-1]+"]");   
	}

	/** Calculates the distance between two (3D) points u and v.
	 *  @param u First point vector.
	 *  @param v Second point vector.
	 *  @return Distance between the two points.
	 */    
	public static float vDistance(float[] u, float[] v)
	{
		return (float)Math.sqrt((v[0] - u[0]) * (v[0] - u[0]) +
								(v[1] - u[1]) * (v[1] - u[1]) +
								(v[2] - u[2]) * (v[2] - u[2]));
	}

	/** Calculates the distance between two (3D) points u and v.
	 *  Double precision version.
	 *  @param u First point vector.
	 *  @param v Second point vector.
	 *  @return Distance between the two points.
	 */    
	public static double vDistance(double[] u, double[] v)
	{
		return Math.sqrt((v[0] - u[0]) * (v[0] - u[0]) +
						 (v[1] - u[1]) * (v[1] - u[1]) +
						 (v[2] - u[2]) * (v[2] - u[2]));
	}

	/** Calculates the squared distance between two (3D) points u and v.
	 *  @param u First point vector.
	 *  @param v Second point vector.
	 *  @return Squared distance between the two points.
	 */    
	public static float vDistanceSq(float[] u, float[] v)
	{
		return ((v[0] - u[0]) * (v[0] - u[0]) +
				(v[1] - u[1]) * (v[1] - u[1]) +
				(v[2] - u[2]) * (v[2] - u[2]));
	}

	/** Calculates the squared distance between two (3D) points u and v.
	 *  Double precision version.
	 *  @param u First point vector.
	 *  @param v Second point vector.
	 *  @return Squared distance between the two points.
	 */    
	public static double vDistanceSq(double[] u, double[] v)
	{
		return ((v[0] - u[0]) * (v[0] - u[0]) +
				(v[1] - u[1]) * (v[1] - u[1]) +
				(v[2] - u[2]) * (v[2] - u[2]));
	}

	/** Calculates the dot product of two (3D) vectors.
	 *  @param u First vector.
	 *  @param v Second vector.
	 *  @return Dot product of the two vectors.
	 */    
	public static float vDot(float[] u, float[] v)
	{
		return (u[0]*v[0] + u[1]*v[1] + u[2]*v[2]);
	}

	/** Calculates the dot product of two (3D) vectors. Double precision version.
	 *  @param u First vector.
	 *  @param v Second vector.
	 *  @return Dot product of the two vectors.
	 */    
	public static double vDot(double[] u, double[] v)
	{
		return (u[0]*v[0] + u[1]*v[1] + u[2]*v[2]);
	}

	/** Creates a (4x4) identity matrix. 
	 *  @return Identity matrix.
	 */
	public static float[][] mIdentity()
	{
		float ident[][] = new float[4][4];
		ident[0][0] = 1;
		ident[1][1] = 1;
		ident[2][2] = 1;
		ident[3][3] = 1;    
		return ident;
	}
	
	/** Creates a (4x4) identity matrix. Double precision version.
	 *  @return Identity matrix.
	 */
	public static double[][] mIdentityd()
	{
		double ident[][] = new double[4][4];
		ident[0][0] = 1;
		ident[1][1] = 1;
		ident[2][2] = 1;
		ident[3][3] = 1;    
		return ident;
	}   

	/** Calculates the inverse of a (4x4) matrix. Double precision version.
	 *  @param m Matrix to find inverse of.
	 *  @return Inverse of matrix.
	 */
	public static double[][] mInverse(double[][] m)
	{
		double[][] inverse = new double[4][4];
		double[]   col     = new double[4];

		LUDecomp ludecomp = new LUDecomp(m);

		for (int j=0; j<4; j++)
		{
			for (int i=0; i<4; i++)
			{
				col[i] = 0.0f;
			}
			col[j]=1.0f;
			ludecomp.luBackSub(col);

			for (int i=0; i<4; i++)
			{
				inverse[i][j] = col[i];
			}
		}
		return inverse;
	}

	/** Calculates the length of a (3D) vector.
	 *  @param v Vector to measure.
	 *  @return Length of the vector.
	 */ 
	public static float vLength(float[] v)
	{
		return (float)(Math.sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
	}

	/** Calculates the length of a (3D) vector. Double precision version.
	 *  @param v Vector to measure.
	 *  @return Length of the vector.
	 */ 
	public static double vLength(double[] v)
	{
		return Math.sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
	}

	/** Calculates the squared length of a (3D) vector.
	 *  @param v Vector to measure.
	 *  @return Squared length of the vector.
	 */ 
	public static float vLengthSq(float[] v)
	{
		return (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
	}

	/** Calculates the squared length of a (3D) vector. Double precision version.
	 *  @param v Vector to measure.
	 *  @return Squared length of the vector.
	 */ 
	public static double vLengthSq(double[] v)
	{
		return (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
	}

	/** Calculates the rotation angle R and axis A such that when rotated, (3D) vector u coincides with
	 *  (3D) vector v.
	 * @param u First normalised vector to compare.
	 * @param v Second normalised vector to compare.
	 * @return Axis vector + rotation angle in radians (4th element in array).
	 */ 
	public static float[] vMatch(float[] u, float[] v)
	{
		float[] axisR = new float[4];
		float[] axis = vCross(u,v);
		axis = vNormalise(axis);
		axisR[0] = axis[0];
		axisR[1] = axis[1];
		axisR[2] = axis[2];
		axisR[3] = (float)(Math.acos(vDot(u,v)));
		return axisR;
	}

	/** Calculates the rotation angle R and axis A such that when rotated, (3D) vector u coincides with
	 *  (3D) vector v. Double precision version.
	 *  @param u First normalised vector to compare.
	 *  @param v Second normalised vector to compare.
	 *  @return Axis vector + rotation angle in radians (4th element in array).
	 */ 
	public static double[] vMatch(double[] u, double[] v)
	{
		double[] axisR = new double[4];
		double[] axis = vCross(u,v);
		axis = vNormalise(axis);
		axisR[0] = axis[0];
		axisR[1] = axis[1];
		axisR[2] = axis[2];
		axisR[3] = Math.acos(vDot(u,v));
		return axisR;
	}

	/** Pre-multiplies two (4x4) homogeneous matrices (m1 dot m2).
	 *  @param m1 First matrix.
	 *  @param m2 Second matrix.
	 *  @return Product of the two matrices.
	 */
	public static float[][] mMultiply(float[][]m1, float[][] m2)
	{
		float mult[][] = new float[4][4];
		mult[0][0] = m1[0][0]*m2[0][0] + m1[0][1]*m2[1][0] + m1[0][2]*m2[2][0];
		mult[0][1] = m1[0][0]*m2[0][1] + m1[0][1]*m2[1][1] + m1[0][2]*m2[2][1];
		mult[0][2] = m1[0][0]*m2[0][2] + m1[0][1]*m2[1][2] + m1[0][2]*m2[2][2];

		mult[1][0] = m1[1][0]*m2[0][0] + m1[1][1]*m2[1][0] + m1[1][2]*m2[2][0];
		mult[1][1] = m1[1][0]*m2[0][1] + m1[1][1]*m2[1][1] + m1[1][2]*m2[2][1];
		mult[1][2] = m1[1][0]*m2[0][2] + m1[1][1]*m2[1][2] + m1[1][2]*m2[2][2];

		mult[2][0] = m1[2][0]*m2[0][0] + m1[2][1]*m2[1][0] + m1[2][2]*m2[2][0];
		mult[2][1] = m1[2][0]*m2[0][1] + m1[2][1]*m2[1][1] + m1[2][2]*m2[2][1];
		mult[2][2] = m1[2][0]*m2[0][2] + m1[2][1]*m2[1][2] + m1[2][2]*m2[2][2];

		mult[3][3] = 1;

		return mult;
	}

	/** Multiplies two 2-dimensional matrices (m1 dot m2) of any size.
	 *  @param m1 First matrix.
	 *  @param m2 Second matrix.
	 *  @return Product of the two matrices or null if the number columns in m1
	 *          is not equal to the number of rows in m2.
	 */
	public static float[][] mmMultiply(float[][]m1, float[][] m2)
	{
		int r1 = m1.length;
		int c1 = m1[0].length;

		int r2 = m2.length;
		int c2 = m2[0].length;

		if (c1 != r2)
		{
			System.err.println("Warning: size of matrices does not permit multiplication");
			return null;
		}

		float mult[][] = new float[r1][c2];

		for (int r=0; r<r1; r++)
		{
			for (int c=0; c<c2; c++)
			{
				mult[r][c] = 0;
				for (int sum=0; sum<c1; sum++)
				{
					mult[r][c] += m1[r][sum]*m2[sum][c];
				}
			}
		}

		return mult;
	}

	/** Multiplies a 2-dimensional matrix by a vector (m dot v) of any size.
	 *  @param m Matrix. This must have the same number of columns as the vector has elements.
	 *  @param v Vector. This must have the same number of elements as the matrix has columns.
	 *  @return m dot v or null if the number columns in m is not equal to the size of v.
	 */
	public static float[] mvMultiply(float[][]m, float[] v)
	{
		int r1 = m.length;
		int c1 = m[0].length;

		int r2 = v.length;

		if (c1 != r2)
		{
			System.err.println("Warning: sizes of matrix and vector do not permit multiplication");
			return null;
		}

		float mult[] = new float[r1];

		for (int r=0; r<r1; r++)
		{
			mult[r] = 0;
			for (int sum=0; sum<c1; sum++)
			{
				mult[r] += m[r][sum]*v[sum];
			}
		}
		return mult;
	}

	/** Pre-multiplies two (4x4) homogeneous matrices (m1 dot m2). Double precision version.
	 *  @param m1 First matrix.
	 *  @param m2 Second matrix.
	 *  @return Product of the two matrices.
	 */
	public static double[][] mMultiply(double[][]m1, double[][] m2)
	{
		double mult[][] = new double[4][4];

		mult[0][0] = m1[0][0]*m2[0][0] + m1[0][1]*m2[1][0] + m1[0][2]*m2[2][0];
		mult[0][1] = m1[0][0]*m2[0][1] + m1[0][1]*m2[1][1] + m1[0][2]*m2[2][1];
		mult[0][2] = m1[0][0]*m2[0][2] + m1[0][1]*m2[1][2] + m1[0][2]*m2[2][2];

		mult[1][0] = m1[1][0]*m2[0][0] + m1[1][1]*m2[1][0] + m1[1][2]*m2[2][0];
		mult[1][1] = m1[1][0]*m2[0][1] + m1[1][1]*m2[1][1] + m1[1][2]*m2[2][1];
		mult[1][2] = m1[1][0]*m2[0][2] + m1[1][1]*m2[1][2] + m1[1][2]*m2[2][2];

		mult[2][0] = m1[2][0]*m2[0][0] + m1[2][1]*m2[1][0] + m1[2][2]*m2[2][0];
		mult[2][1] = m1[2][0]*m2[0][1] + m1[2][1]*m2[1][1] + m1[2][2]*m2[2][1];
		mult[2][2] = m1[2][0]*m2[0][2] + m1[2][1]*m2[1][2] + m1[2][2]*m2[2][2];

		mult[3][3] = 1;

		return mult;
	}
	
	/** Multiplies two 2-dimensional matrices (m1 dot m2) of any size. Double precision version.
	 *  @param m1 First matrix.
	 *  @param m2 Second matrix.
	 *  @return Product of the two matrices or null if the number columns in m1
	 *          is not equal to the number of rows in m2.
	 */
	public static double[][] mmMultiply(double[][]m1, double[][] m2)
	{
		int r1 = m1.length;
		int c1 = m1[0].length;

		int r2 = m2.length;
		int c2 = m2[0].length;

		if (c1 != r2)
		{
			System.err.println("Warning: size of matrices does not permit multiplication");
			return null;
		}

		double mult[][] = new double[r1][c2];

		for (int r=0; r<r1; r++)
		{
			for (int c=0; c<c2; c++)
			{
				mult[r][c] = 0;
				for (int sum=0; sum<c1; sum++)
				{
					mult[r][c] += m1[r][sum]*m2[sum][c];
				}
			}
		}
		return mult;
	}
	
	/** Multiplies a 2-dimensional matrix by a vector (m dot v) of any size. Double precision version.
	 *  @param m Matrix. This must have the same number of columns as the vector has elements.
	 *  @param v Vector. This must have the same number of elements as the matrix has columns.
	 *  @return m dot v or null if the number columns in m is not equal to the size of v.
	 */
	public static double[] mvMultiply(double[][]m, double[] v)
	{
		int r1 = m.length;
		int c1 = m[0].length;

		int r2 = v.length;

		if (c1 != r2)
		{
			System.err.println("Warning: sizes of matrix and vector do not permit multiplication");
			return null;
		}

		double mult[] = new double[r1];

		for (int r=0; r<r1; r++)
		{
			mult[r] = 0;
			for (int sum=0; sum<c1; sum++)
			{
				mult[r] += m[r][sum]*v[sum];
			}
		}
		return mult;
	}

	/** Normalises a (3D) vector so its length is 1.
	 *  @param v Vector to normalise.
	 *  @return Normalised vector.
	 */
	public static float[] vNormalise(float[] v)
	{
		float[] normal = new float[3];
		float length = vLength (v);
		if (length != 0)
		{
			normal = vScale(v, 1f/length);
		}
		else
		{
			System.err.println("Warning: Can't normalise a zero length vector");
		}
		return normal;
	}

	/** Normalises a (3D) vector so its length is 1. Double precision version.
	 *  @param v Vector to normalise.
	 *  @return Normalised vector.
	 */
	public static double[] vNormalise(double[] v)
	{
		double[] normal = new double[3];
		double length = vLength (v);
		if (length != 0)
		{
			normal = vScale (v, 1.0/length);
		}
		else
		{
			System.err.println("Warning: Can't normalise a zero length vector");
		}
		return normal;
	}

	/** Calculates the (4x4) homogeneous rotation matrix by a given angle about a given axis.
	 *  @param axis Vector representing normalised rotation axis.
	 *  @param alpha Angle to rotate by (in radians).
	 *  @return Rotation matrix.
	 */
	public static float[][] mRotate(float[] axis, float alpha)
	{
		float   c = (float)Math.cos(alpha);
		float   s = (float)Math.sin(alpha);
		float   t = 1 - c;
		float   x = axis[0],
		y = axis[1],
		z = axis[2];
		float   x2 = x*x,
		y2 = y*y,
		z2 = z*z,
		xy = x*y,
		xz = x*z,
		yz = y*z,
		sx = s*x,
		sy = s*y,
		sz = s*z;

		float[][] R = new float[4][4];

		R[0][0] = (t * x2) + c;
		R[1][0] = (t * xy) - (sz);
		R[2][0] = (t * xz) + (sy);

		R[0][1] = (t * xy) + (sz);
		R[1][1] = (t * y2) + c;
		R[2][1] = (t * yz) - (sx);

		R[0][2] = (t * xz) - (sy);
		R[1][2] = (t * yz) + (sx);
		R[2][2] = (t * z2) + c;

		R[3][3] = 1;

		return R;
	}

	/** Calculates the (4x4) homogeneous rotation matrix by a given angle about a given axis. 
	 *  Double precision version.
	 *  @param axis Vector representing normalised rotation axis.
	 *  @param alpha Angle to rotate by (in radians).
	 *  @return Rotation matrix.
	 */
	public static double[][] mRotate(double[] axis, double alpha)
	{
		double  c = (float)Math.cos(alpha);
		double  s = (float)Math.sin(alpha);
		double  t = 1 - c;
		double  x = axis[0],
		y = axis[1],
		z = axis[2];
		double  x2 = x*x,
		y2 = y*y,
		z2 = z*z,
		xy = x*y,
		xz = x*z,
		yz = y*z,
		sx = s*x,
		sy = s*y,
		sz = s*z;

		double[][] R = new double[4][4];

		R[0][0] = (t * x2) + c;
		R[1][0] = (t * xy) - (sz);
		R[2][0] = (t * xz) + (sy);

		R[0][1] = (t * xy) + (sz);
		R[1][1] = (t * y2) + c;
		R[2][1] = (t * yz) - (sx);

		R[0][2] = (t * xz) - (sy);
		R[1][2] = (t * yz) + (sx);
		R[2][2] = (t * z2) + c;

		R[3][3] = 1;

		return R;
	}

	/** Rotates a (3D) vector by a given angle around a given axis.
	 *  @param v Vector to rotate.
	 *  @param axis Vector representing normalised rotation axis.
	 *  @param alpha Angle to rotate by (in radians).
	 *  @return Rotated vector.
	 */
	public static float[] vRotate(float[] v, float[] axis, float alpha)
	{
		float[][] R    = mRotate(axis,alpha);
		float[] rotate = new float[3];

		rotate[0] = (v[0]*R[0][0]) + (v[1]*R[1][0]) + (v[2]*R[2][0]);
		rotate[1] = (v[0]*R[0][1]) + (v[1]*R[1][1]) + (v[2]*R[2][1]);
		rotate[2] = (v[0]*R[0][2]) + (v[1]*R[1][2]) + (v[2]*R[2][2]);
		return rotate;
	}

	/** Rotates a (3D) vector by a given angle around a given axis. Double precision version.
	 *  @param v Vector to rotate. Double precision version.
	 *  @param axis Vector representing normalised rotation axis.
	 *  @param alpha Angle to rotate by (in radians).
	 *  @return Rotated vector.
	 */
	public static double[] vRotate(double[] v, double[] axis, double alpha)
	{
		double[][] R    = mRotate(axis,alpha);
		double[] rotate = new double[3];

		rotate[0] = (v[0]*R[0][0]) + (v[1]*R[1][0]) + (v[2]*R[2][0]);
		rotate[1] = (v[0]*R[0][1]) + (v[1]*R[1][1]) + (v[2]*R[2][1]);
		rotate[2] = (v[0]*R[0][2]) + (v[1]*R[1][2]) + (v[2]*R[2][2]);

		return rotate;
	}

	/** Rotates a (3D) vector by a given angle around the X axis.
	 *  @param v Vector to rotate.
	 *  @param alpha Angle to rotate by (in radians).
	 *  @return Rotated vector.
	 */
	public static float[] vRotateX(float[] v, float alpha)
	{  
		float s = (float)Math.sin(alpha);
		float c = (float)Math.cos(alpha);
		float[] rotate = new float[3];
		rotate[0] =  v[0];
		rotate[1] =  v[1]*c + v[2]*s;
		rotate[2] = -v[1]*s + v[2]*c;
		return rotate;
	}

	/** Rotates a (3D) vector by a given angle around the X axis. Double precision version.
	 *  @param v Vector to rotate.
	 *  @param alpha Angle to rotate by (in radians).
	 *  @return Rotated vector.
	 */
	public static double[] vRotateX(double[] v, double alpha)
	{
		double s = Math.sin(alpha);
		double c = Math.cos(alpha);
		double[] rotate = new double[3];
		rotate[0] =  v[0];
		rotate[1] =  v[1]*c + v[2]*s;
		rotate[2] = -v[1]*s + v[2]*c;
		return rotate;
	}

	/** Rotates a (3D) vector by a given angle around the Y axis.
	 *  @param v Vector to rotate.
	 *  @param alpha Angle to rotate by (in radians).
	 *  @return Rotated vector.
	 */
	public static float[] vRotateY(float[] v, float alpha)
	{
		float s = (float)Math.sin(alpha);
		float c = (float)Math.cos(alpha);
		float[] rotate = new float[3];
		rotate[0] = v[0]*c - v[2]*s;
		rotate[1] = v[1];
		rotate[2] = v[0]*s + v[2]*c;
		return rotate;
	}

	/** Rotates a (3D) vector by a given angle around the Y axis. Double precision version.
	 *  @param v Vector to rotate.
	 *  @param alpha Angle to rotate by (in radians).
	 *  @return Rotated vector.
	 */
	public static double[] vRotateY(double[] v, double alpha)
	{
		double s = Math.sin(alpha);
		double c = Math.cos(alpha);
		double[] rotate = new double[3];
		rotate[0] = v[0]*c - v[2]*s;
		rotate[1] = v[1];
		rotate[2] = v[0]*s + v[2]*c;
		return rotate;
	}

	/** Rotates a (3D) vector by a given angle around the Z axis.
	 *  @param v Vector to rotate.
	 *  @param alpha Angle to rotate by (in radians).
	 *  @return Rotated vector.
	 */
	public static float[] vRotateZ(float[] v, float alpha)
	{  
		float s = (float)Math.sin(alpha);
		float c = (float)Math.cos(alpha);
		float[] rotate = new float[3];
		rotate[0] = v[0]*c + v[1]*s;
		rotate[1] = -v[0]*s + v[1]*c;
		rotate[2] = v[2];
		return rotate;
	}

	/** Rotates a (3D) vector by a given angle around the Z axis. Double precision version.
	 *  @param v Vector to rotate.
	 *  @param alpha Angle to rotate by (in radians).
	 *  @return Rotated vector.
	 */
	public static double[] vRotateZ(double[] v, double alpha)
	{
		double s = Math.sin(alpha);
		double c = Math.cos(alpha);
		double[] rotate = new double[3];
		rotate[0] = v[0]*c + v[1]*s;
		rotate[1] = -v[0]*s + v[1]*c;
		rotate[2] = v[2];
		return rotate;
	}

	/** Given two rotation axis vectors and their angles of rotation, returns
	 *  the relative rotation vector that would rotate the first to the second.
	 *  @param v1 First rotation axis.
	 *  @param alpha1 Angle of first rotation (in radians).
	 *  @param v2 Second rotation axis.
	 *  @param alpha2 Angle of second rotation (in radians).
	 *  @return Relative rotation vector (axis + angle as fourth element in array)
	 */
	public static double[] vRotateRel(double[] v1, double alpha1, double[] v2, double alpha2)
	{
		// Convert rotation vectors into rotation matrices.
		double[][] m1 = mRotate(v1,alpha1);
		double[][] m2 = mRotate(v2,alpha2);

		// Find the inverse of the first matrix and pre-multiply it be the second.
		double[][] m1inv = mInverse(m1);
		double[][] R = mMultiply(m2,m1inv); // Relative rotation matrix.

		// Find an arbitrary non-co-linear vector.
		double[] u = new double[3];
		u[0] = v1[1]/2.0;
		u[1] = v1[2]/3.0;
		u[2] = v1[0]/4.0; 
		double[] u1 = vNormalise(u);

		// Cross product of u1 and v1 will be perpendicular to both.
		u1 = Matrix.vCross(v1,u1);

		// Transform the arbitrary perpendicular vector by the relative rotation.
		double[] u2 = vTransform(R,u1);

		// Use vMatch to convert into a rotation vector.
		double[] axisR = vMatch(u1,u2);

		return axisR;
	}

	/** Multiplies a (3D) vector by a given scalar constant.
	 *  @param v Vector to scale.
	 *  @param s Scalar constant.
	 *  @return Scaled vector.
	 */
	public static float[] vScale(float[] v, float s)
	{
		float scale[] = new float[3];
		scale[0] = s*v[0];
		scale[1] = s*v[1];
		scale[2] = s*v[2];
		return scale;
	}

	/** Multiplies a (3D) vector by a given scalar constant. Double precision version.
	 *  @param v Vector to scale.
	 *  @param s Scalar constant.
	 *  @return Scaled vector.
	 */
	public static double[] vScale(double[] v, double s)
	{
		double scale[] = new double[3];
		scale[0] = s*v[0];
		scale[1] = s*v[1];
		scale[2] = s*v[2];
		return scale;
	}

	/** Finds the (3D) vector represented by given phi and lambda angles in spherical coordinates. 
	 *  @param phi 'Latitude' in radians from 0 - PI.
	 *  @param lambda 'Longitude' in radians from 0 - 2*PI.
	 *  @return Normalised vector representation of spherical coordinates.
	 */
	public static float[] vSphere(float phi, float lambda)
	{
		float sphere[] = new float[3];
		sphere[0] = (float)(Math.sin(phi)*Math.cos(lambda));
		sphere[1] = (float)(Math.sin(phi)*Math.sin(lambda));
		sphere[2] = (float)(Math.cos(phi));
		return sphere;
	}

	/** Finds the (3D) vector represented by given phi and lambda angles in spherical coordinates. 
	 *  Double precision version. 
	 *  @param phi 'Latitude' in radians from 0 - PI.
	 *  @param lambda 'Longitude' in radians from 0 - 2*PI.
	 *  @return Normalised vector representation of spherical coordinates.
	 */
	public static double[] vSphere(double phi, double lambda)
	{
		double sphere[] = new double[3];
		sphere[0] = Math.sin(phi)*Math.cos(lambda);
		sphere[1] = Math.sin(phi)*Math.sin(lambda);
		sphere[2] = Math.cos(phi);
		return sphere;
	}

	/** Subtracts a (3D) vector from another.
	 *  @param u First vector.
	 *  @param v Second vector.
	 *  @return Difference between the two vectors (u-v).
	 */
	public static float[] vSubtract(float[] u, float[] v)
	{ 
		float sub[] = new float[3];
		sub[0] = u[0] - v[0];
		sub[1] = u[1] - v[1];
		sub[2] = u[2] - v[2];
		return sub;
	}

	/** Subtracts a (3D) vector from another. Double precision version.
	 *  @param u First vector.
	 *  @param v Second vector.
	 *  @return Difference between the two vectors (u-v).
	 */
	public static double[] vSubtract(double[] u, double[] v)
	{ 
		double sub[] = new double[3];
		sub[0] = u[0] - v[0];
		sub[1] = u[1] - v[1];
		sub[2] = u[2] - v[2];
		return sub;
	}

	/** Pre-multiplies a (3D) vector by a homogeneous (4x4) transformation matrix. Will treat the coordinate 
	 *  vector as either a 1x3 matrix, or its 1x4 homogeneous equivalent.
	 *  @param m Transformation Matrix (4x4).
	 *  @param v Coordinate vector.
	 *  @return Transformed coordinate vector.
	 */
	public static float[] vTransform(float[][]m, float[] v)
	{
		float transform[] = new float[3];
		transform[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
		transform[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
		transform[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
		return transform;
	}

	/** Pre-multiplies a (3D) vector by a homogeneous (4x4) transformation matrix. Will treat the coordinate 
	 *  vector as either a 1x3 matrix, or its 1x4 homogeneous equivalent. Double precision version.
	 *  @param m Transformation Matrix (4x4).
	 *  @param v Coordinate vector.
	 *  @return Transformed coordinate vector.
	 */
	public static double[] vTransform(double[][]m, double[] v)
	{
		double transform[] = new double[3];
		transform[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
		transform[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
		transform[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
		return transform;
	}

	/** Returns a 1-dimensional 'serialised' version of a given 2d 4x4 matrix.
	 *  @param m 4x4 matrix to serialise.
	 *  @return 16 1d vector representation.
	 */
	public static float[] serialise(float[][]m)
	{
		int i=0;
		float[] sm = new float[16];
		for (int r=0; r<4; r++)
		{
			for (int c=0; c<4; c++)
			{
				sm[i++] = m[r][c];
			}
		}
		return sm;
	}

	/** Returns a 1-dimensional 'serialised' version of a given 2d 4x4 matrix. Double precision version.
	 *  @param m 4x4 matrix to serialise.
	 *  @return 16 1d vector representation.
	 */
	public static double[] serialise(double[][]m)
	{
		int i=0;
		double[] sm = new double[16];
		for (int r=0; r<4; r++)
		{ 
			for (int c=0; c<4; c++)
			{
				sm[i++] = m[r][c];
			}
		}
		return sm;
	}
}  